/***********
   寻迹小车
   两个灰度测试版
   目前沿线走的代码应该实现了
*/

//定义一个转向延时时间
#define TRUN_TIME 100

//定义两个车轮的最大速度
#define LEFT_MAX 150
#define RIGHT_MAX 105

//引脚改名，好记
#define PWM_LEFT 5
#define PWM_RIGHT 6
#define STBY 7
#define AIN1 8
#define AIN2 9
#define BIN1 10
#define BIN2 11

#define POWER_SIGN_LEFT 12
#define POWER_SIGN_RIGHT 13

#define SIGN_LEFT 14
#define SIGN_RIGHT 15

//左转中断优先
#define INT_LEFT 2
#define INT_RIGHT 3

//定义一些函数
void TRUN_LEFT();
void TRUN_RIGHT();
//小车的走路代码，冲鸭！！！！
void CHONGYA(int left, int right);

//定义转向使用时间变量
int USING_TIME;

void setup() {
  Serial.begin(9600);
  //偷懒用循环初始化引脚模式,注意是否从引脚5开始(测试一下)
  for (int i = 5; i <= 13; i++)
    pinMode(i, OUTPUT);
  //初始化信号引脚
  pinMode(SIGN_LEFT, INPUT);
  pinMode(SIGN_RIGHT, INPUT);

  //设置中断,低电平触发
  /* 事实上低电平触发无法达到逾预期结果 */
  /* 猜测可能是由于初始化的时候传感器处于低电平 */
  /* 直接进入中断了 */
  attachInterrupt(digitalPinToInterrupt(INT_LEFT), TRUN_RIGHT, FALLING);
  attachInterrupt(digitalPinToInterrupt(INT_RIGHT), TRUN_LEFT, FALLING);
  for (int i = 8; i <= 11; i++) digitalWrite(i, 0);
  digitalWrite(STBY, 1);

  digitalWrite(POWER_SIGN_LEFT, 1);
  digitalWrite(POWER_SIGN_RIGHT, 1);

  delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:

  CHONGYA(LEFT_MAX, RIGHT_MAX);
}

void CHONGYA(int left, int right)
{
  digitalWrite(AIN1, 0);
  digitalWrite(AIN2, 1);
  digitalWrite(BIN1, 0);
  digitalWrite(BIN2, 1);

  analogWrite(PWM_LEFT, left);
  analogWrite(PWM_RIGHT, right);
  delay(TRUN_TIME);
}

void TRUN_LEFT()
{
  //Serial.println(digitalRead(INT_LEFT));
  Serial.println("LEFT");
  int rare = LEFT_MAX;

  while (digitalRead(INT_RIGHT) == 1)
  {
    CHONGYA(rare , RIGHT_MAX);

    //先写一个rare--吧，应该不至于溢出
    if (rare > 0) rare--;
    else rare = 0;
  }
}

void TRUN_RIGHT()
{
  Serial.println("RIGHT");
  int rare = RIGHT_MAX;
  while (digitalRead(INT_LEFT) == 1 )
  {
    CHONGYA(LEFT_MAX, rare);
    if (rare > 0) rare--;
    else rare = 0;
  }
}
